#include "../inc/kinetic_control.h"
#include "sensor_inven_mpu6xxx.h"
//#include "arm_math.h"
#include <math.h>

#define CONTROL_THREAD_PRO 3   //优先级高一点
#define CONTROL_THREAD_STACK_SIZE 2048
#define CONTROL_MPU_STACK_SIZE 512
#define CONTROL_THREAD_RUNTIME 5
#define MPU_HAND_THREAD_RUNTIME 20
#define CONTROL_TICK 20 //电机控制周期

#define PWM_PERIOD 100000


//MPU6050 系数设置
#define C_FITTER 0.15f   //一节互补系数
#define PI 3.14159f
#define DT 0.005f
#define GROY_CONVER 2000.0f / 32768.0f
#define ACC_CONVER 2.0*9.8f / 32768.0f

//#define __GET_CURRENT_ENCODER(n) {TIM##(n)->cnt}
//#define GET_CURRENT_ENCODER __GET_CURRENT_ENCODER((n))




#define DEBUG_PID 1

static PID_TypDef motor_pid[4]; //电机控制pid
static rt_thread_t ctr_tid;
static rt_thread_t update_tid;
static rt_thread_t mpu_handler_tid;
static rt_device_t encoder[4];
static struct rt_device_pwm *pwm_dev;
static struct mpu6xxx_device *mpu6050;
static struct mpu6xxx_3axes gyro_axes,acc_axes;
sendWave_t wave_data;



volatile float x_speed,y_speed,z_speed;
#if DEBUG_PID
    volatile int targetEncoder_test;
    volatile int m_n = 0;
#endif

int v2encoder(float v)
{

    return 0;
}


/*
 *  陀螺仪数据处理线程
 *无参数输入
 * */
void mpu_handler(void *parm)
{

   float math_pitch = 0.0f,math_yaw = 0.0f,math_roll = 0.0f;
   float last_angle_yaw,last_angle_pitch = 0.0f,last_angle_roll;

   float temp_g_x = 0.0f,temp_g_y = 0.0f,temp_g_z = 0.0f;   /*临时变量*/
   float temp_a_x = 0.0f,temp_a_y = 0.0f,temp_a_z = 0.0f;


   int8_t first = 1;    //获取第1次的误差值
   float zero_pitch = 0.0f;    //pitch角度偏差
   float zero_yaw = 0.0f;
   float zero_roll = 0.0f;




    while(1){
        mpu6xxx_get_gyro(mpu6050,&gyro_axes);
        mpu6xxx_get_accel(mpu6050, &acc_axes);

        wave_data.groy_x = (gyro_axes.x);
        wave_data.groy_y = (gyro_axes.y);
        wave_data.groy_z = (gyro_axes.z);


        wave_data.acc_x = (acc_axes.x);
        wave_data.acc_y = (acc_axes.y);
        wave_data.acc_z = (acc_axes.z);




        temp_a_x = (float)wave_data.acc_x * ACC_CONVER;
        temp_a_y = (float)wave_data.acc_y * ACC_CONVER;
        temp_a_z = (float)wave_data.acc_z * ACC_CONVER;

        temp_g_x = (float) wave_data.groy_x * GROY_CONVER;
        temp_g_y = (float) wave_data.groy_y * GROY_CONVER;
        temp_g_z = (float) wave_data.groy_z * GROY_CONVER;

        //org_groyy[0] =


        math_pitch = -atan2f(temp_a_x,temp_a_z) * 180.0 / PI;
        math_roll = atan2f(temp_a_y,temp_a_z) * 180.0 / PI;

        math_yaw = math_yaw + temp_g_z * DT;    //存在积分误差

        math_pitch = C_FITTER * math_pitch + (1 - C_FITTER) * (last_angle_pitch + DT * temp_g_y);    /*加速度和计算出来的角度不一样……*/
        last_angle_pitch = math_pitch;

        math_roll =  C_FITTER * math_roll + (1 - C_FITTER) * (last_angle_roll + DT * temp_g_x);
        last_angle_roll = math_roll;

        if(first)
        {
            zero_pitch = math_pitch;
            //zero_roll = math_roll;        //蜜汁bug
            first = 0;
        }

        wave_data.pitch = (int)(math_pitch - zero_pitch);
        wave_data.roll = (int)(math_roll-zero_roll);
        wave_data.yaw = (int)(math_yaw - zero_yaw);





        rt_thread_delay(5);  /*给10秒的时序*/
    }
}

void kinetic_calc(float x,float y,float z,float f_out[4])
{
    float v1 = 0.0f,v2 = 0.0f, v3 = 0.0f, v4 = 0.0f;    //第1~4个轮子的速度,单位是m/s

    v1 = y - x + z * (DIS_A + DIS_B);
    v2 = y + x - z * (DIS_A + DIS_B);
    v3 = y - x - z * (DIS_A + DIS_B);
    v4 = y + x + z * (DIS_A + DIS_B);

    f_out[0] = v2encoder(v1);
    f_out[1] = v2encoder(v2);
    f_out[2] = v2encoder(v3);
    f_out[3] = v2encoder(v4);

    return;
}

void kinetic_control(void * parm)
{
    float target_encoder[4];
    float outpwm[4];
    rt_int32_t encoder_value[4] = {0};


    while(1)
    {
        rt_thread_mdelay(CONTROL_TICK);      //挂起20ms


        rt_enter_critical();
        kinetic_calc(x_speed, y_speed, z_speed, target_encoder);        //获取电机目标




#if DEBUG_PID
        rt_device_read(encoder[0], 0, &encoder_value[0], 1);
        rt_device_read(encoder[1], 0, &encoder_value[1], 1);
        rt_device_read(encoder[2], 0, &encoder_value[2], 1);
        rt_device_read(encoder[3], 0, &encoder_value[3], 1);
        //rt_kprintf("Read value:%d \n",encoder_value);
        //outpwm[m_n] += IncPID(0,targetEncoder_test, &motor_pid[m_n]);
        rt_device_control(encoder[0], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);
        rt_device_control(encoder[1], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);
        rt_device_control(encoder[2], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);
        rt_device_control(encoder[3], PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);


        wave_data.encoder_data[0] = encoder_value[0];
        wave_data.encoder_data[1] = encoder_value[1];
        wave_data.encoder_data[2] = encoder_value[2];
        wave_data.encoder_data[3] = encoder_value[3];


        //rt_mb_send(wave_data_mailbox,(rt_ubase_t *)&wave_data);

        //rt_kprintf("Read value:%d \n",encoder_value[0]);

#else

        outpwm[0] += IncPID(encoder[0], v2encoder(target_encoder[0]), &motor_pid[0]);
        outpwm[1] += IncPID(encoder[1], v2encoder(target_encoder[1]), &motor_pid[1]);
        outpwm[2] += IncPID(encoder[2], v2encoder(target_encoder[2]), &motor_pid[2]);
        outpwm[3] += IncPID(encoder[3], v2encoder(target_encoder[3]), &motor_pid[3]);

#endif
        //输出PWM

        rt_exit_critical();

    }

}

void update_newvalue_thread(void *parm)
{
    sendPackage_t *new_package = NULL;

    while(1)
    {
        rt_mb_recv(package_mailbox, (rt_ubase_t) &new_package, RT_WAITING_FOREVER);

        x_speed = new_package->v_x;
        y_speed = new_package->v_y;
        z_speed = new_package->v_z;

        m_n = new_package->m_n;
        targetEncoder_test = new_package->target_encoder;
        PID_Set(&motor_pid[m_n], new_package->kp, new_package->ki, new_package->kd);

        rt_kprintf("%d",new_package->m_n);
    }
}


rt_err_t control_init(void)
{

    PID_init(&motor_pid[0]);
    PID_init(&motor_pid[1]);
    PID_init(&motor_pid[2]);
    PID_init(&motor_pid[3]);

    encoder[0] = rt_device_find("pulse2");
    encoder[1] = rt_device_find("pulse3");
    encoder[2] = rt_device_find("pulse4");
    encoder[3] = rt_device_find("pulse5");


    if(rt_device_open(encoder[0],RT_DEVICE_OFLAG_RDONLY) != RT_EOK)
    {
        rt_kprintf("Cannot open pulse encoder0");
        return RT_ERROR;
    }
    if(rt_device_open(encoder[1],RT_DEVICE_OFLAG_RDONLY) != RT_EOK)
    {
        rt_kprintf("Cannot open pulse encoder1");
        return RT_ERROR;
    }
    if(rt_device_open(encoder[2],RT_DEVICE_OFLAG_RDONLY) != RT_EOK)
    {
        rt_kprintf("Cannot open pulse encoder2");
        return RT_ERROR;
    }
    if(rt_device_open(encoder[3],RT_DEVICE_OFLAG_RDONLY) != RT_EOK)
    {
        rt_kprintf("Cannot open pulse encoder3");
        return RT_ERROR;
    }

    pwm_dev = (struct rt_device_pwm *)rt_device_find("pwm8");

    if(pwm_dev == RT_NULL)
    {
        rt_kprintf("Cannot open pwm8 \r\n");
        return RT_NULL;
    }
    else{
        rt_kprintf("open pwm8 sucessfully\r\n");
    }


    rt_pwm_set(pwm_dev, 1, PWM_PERIOD, 0);
    rt_pwm_set(pwm_dev, 2, PWM_PERIOD, 0);
    rt_pwm_set(pwm_dev, 3, PWM_PERIOD, 0);
    rt_pwm_set(pwm_dev, 4, PWM_PERIOD, 0);

    rt_pwm_enable(pwm_dev, 1);
    rt_pwm_enable(pwm_dev, 2);
    rt_pwm_enable(pwm_dev, 3);
    rt_pwm_enable(pwm_dev, 4);


    /*
     * 下面mpu6050初始化代码为错误代码
     * */

//    mpu6050.intf.dev_name = "i2c1";
//    mpu6050.intf.user_data = (void *)MPU6XXX_ADDR_DEFAULT;
//    mpu6050.irq_pin.pin = RT_PIN_NONE;

//    rt_hw_mpu6xxx_init("mpu", &mpu6050);



    ctr_tid = rt_thread_create("kinetic control", kinetic_control, NULL, CONTROL_THREAD_STACK_SIZE, CONTROL_THREAD_PRO, CONTROL_THREAD_RUNTIME);
    update_tid = rt_thread_create("Update value", update_newvalue_thread, NULL, CONTROL_THREAD_STACK_SIZE, CONTROL_THREAD_PRO+1, CONTROL_THREAD_RUNTIME);

    if(ctr_tid != RT_NULL){
        //rt_thread_startup(ctr_tid);
        rt_kprintf("Startup control thread successfully \r\n");
    }

    if(update_tid != RT_NULL){
        rt_thread_startup(update_tid);
        rt_kprintf("Startup update value thread successfully \r\n");
    }


    mpu6050 = mpu6xxx_init("i2c1", RT_NULL);



    if(mpu6050 != RT_NULL)
    {
        mpu6xxx_set_param(mpu6050, MPU6XXX_ACCEL_RANGE, MPU6XXX_GYRO_RANGE_2000DPS);  //陀螺仪范围配置
        mpu6xxx_set_param(mpu6050, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_2G);     //加速度计，一般设置为±2G
        mpu6xxx_set_param(mpu6050, MPU6XXX_SAMPLE_RATE, 50);                       //采样频率
        mpu6xxx_set_param(mpu6050, MPU6XXX_DLPF_CONFIG, 25);                       //数字低通滤波器设置，一般为1/2采样率

        mpu_handler_tid = rt_thread_create("Mpu handler", mpu_handler, NULL, CONTROL_MPU_STACK_SIZE, CONTROL_THREAD_PRO, CONTROL_THREAD_RUNTIME);
        if(mpu_handler_tid != RT_NULL)
        {
            rt_thread_startup(mpu_handler_tid);
            rt_kprintf("Startup mpu_handler thread successfully \r\n");

        }
    }
    else
        rt_kprintf("mpu6050 init error!\r\n");

    return RT_EOK;
}
MSH_CMD_EXPORT(control_init,startup control thread);
